﻿using System;
using System.Collections.Generic;
using System.Linq;

public class DpGeospatialCalculator
{
    // 地球参数 (WGS84)
    private const double EarthRadius = 6378137.0; // 赤道半径 (m)
    private const double EarthEccentricitySquared = 6.69437999014e-3;

    // 相机内参
    private const int ImageWidth = 640;
    private const int ImageHeight = 512;
    private const double FocalLength = 0.024; // 焦距 (m)
    private const double PixelSize = 12e-6;   // 像元尺寸 (m)
    private static readonly double Fx = FocalLength / PixelSize; // 像素单位
    private static readonly double Fy = FocalLength / PixelSize;
    private static readonly double Cx = ImageWidth / 2.0;
    private static readonly double Cy = ImageHeight / 2.0;

    public static void LMain()
    {
        // 输入参数
        double droneRoll = 15.0, dronePitch = -18.4, droneYaw = 88.90;
        double gimbalRoll = 180.0, gimbalPitch = -90.0, gimbalYaw = -89.30;
        double droneLon = 100.20005, droneLat = 36.003380555555559, droneAlt = 3109.71;
        double relativeHeight = 69.662; // 相对高度 (m)
        double groundAlt = droneAlt - relativeHeight; // 地面海拔高度

        // 目标物像素坐标 [中心点]
        double targetCenterX = (169 + 192) / 2.0;
        double targetCenterY = (253 + 296) / 2.0;

        // 计算图片中心点坐标
        var imageCenter = CalculateGroundPoint(
            new Point2D(Cx, Cy),
            droneRoll, dronePitch, droneYaw,
            gimbalRoll, gimbalPitch, gimbalYaw,
            droneLon, droneLat, droneAlt,
            groundAlt
        );

        // 计算目标物中心点坐标
        var targetCenter = CalculateGroundPoint(
            new Point2D(targetCenterX, targetCenterY),
            droneRoll, dronePitch, droneYaw,
            gimbalRoll, gimbalPitch, gimbalYaw,
            droneLon, droneLat, droneAlt,
            groundAlt
        );

        // 输出结果
        Console.WriteLine("图片中心经纬度:");
        Console.WriteLine($"经度: {imageCenter.Longitude:F10}, 纬度: {imageCenter.Latitude:F10}");
        Console.WriteLine("\n目标物中心经纬度:");
        Console.WriteLine($"经度: {targetCenter.Longitude:F10}, 纬度: {targetCenter.Latitude:F10}");
    }

    private static GeoPoint CalculateGroundPoint(
        Point2D pixel,
        double droneRoll, double dronePitch, double droneYaw,
        double gimbalRoll, double gimbalPitch, double gimbalYaw,
        double droneLon, double droneLat, double droneAlt,
        double groundAlt)
    {
        // 1. 计算相机坐标系中的射线向量
        var rayCamera = new Vector3D(
            (pixel.X - Cx) / Fx,
            (pixel.Y - Cy) / Fy,
            1.0
        );

        // 2. 组合无人机和云台旋转矩阵
        var R_drone = RotationMatrix(droneRoll, dronePitch, droneYaw);
        var R_gimbal = RotationMatrix(gimbalRoll, gimbalPitch, gimbalYaw);
        var R_combined = MatrixMultiply(R_drone, R_gimbal);

        // 3. 转换到NED坐标系
        var rayNed = MatrixVectorMultiply(R_combined, rayCamera);
        rayNed = Normalize(rayNed);

        // 4. 计算射线与地面的交点 (平面近似)
        double t = (droneAlt - groundAlt) / rayNed.Z;
        double deltaNorth = t * rayNed.Y;
        double deltaEast = t * rayNed.X;

        // 5. 转换到地理坐标系
        return NedToGeodetic(deltaNorth, deltaEast, droneLon, droneLat, droneAlt);
    }

    private static double[,] RotationMatrix(double roll, double pitch, double yaw)
    {
        double cr = Math.Cos(DegToRad(roll));
        double sr = Math.Sin(DegToRad(roll));
        double cp = Math.Cos(DegToRad(pitch));
        double sp = Math.Sin(DegToRad(pitch));
        double cy = Math.Cos(DegToRad(yaw));
        double sy = Math.Sin(DegToRad(yaw));

        return new double[,] {
            { cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr },
            { sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr },
            { -sp,     cp * sr,                cp * cr               }
        };
    }

    private static double[,] MatrixMultiply(double[,] a, double[,] b)
    {
        int rows = a.GetLength(0);
        int cols = b.GetLength(1);
        double[,] result = new double[rows, cols];

        for (int i = 0; i < rows; i++)
        {
            for (int j = 0; j < cols; j++)
            {
                result[i, j] = 0;
                for (int k = 0; k < a.GetLength(1); k++)
                {
                    result[i, j] += a[i, k] * b[k, j];
                }
            }
        }
        return result;
    }

    private static Vector3D MatrixVectorMultiply(double[,] m, Vector3D v)
    {
        return new Vector3D(
            m[0, 0] * v.X + m[0, 1] * v.Y + m[0, 2] * v.Z,
            m[1, 0] * v.X + m[1, 1] * v.Y + m[1, 2] * v.Z,
            m[2, 0] * v.X + m[2, 1] * v.Y + m[2, 2] * v.Z
        );
    }

    private static Vector3D Normalize(Vector3D v)
    {
        double length = Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
        return new Vector3D(v.X / length, v.Y / length, v.Z / length);
    }

    private static GeoPoint NedToGeodetic(double deltaNorth, double deltaEast, double lon0, double lat0, double alt0)
    {
        double latRad = DegToRad(lat0);
        double cosLat = Math.Cos(latRad);
        double sinLat = Math.Sin(latRad);

        // 曲率半径
        double Rn = EarthRadius / Math.Sqrt(1 - EarthEccentricitySquared * sinLat * sinLat);
        double Rm = Rn * (1 - EarthEccentricitySquared) / (1 - EarthEccentricitySquared * sinLat * sinLat);

        // 计算纬度变化 (弧度)
        double deltaLat = deltaNorth / (Rm + alt0);

        // 计算经度变化 (弧度)
        double deltaLon = deltaEast / ((Rn + alt0) * cosLat);

        return new GeoPoint(
            lon0 + RadToDeg(deltaLon),
            lat0 + RadToDeg(deltaLat)
        );
    }

    private static double DegToRad(double deg) => deg * Math.PI / 180.0;
    private static double RadToDeg(double rad) => rad * 180.0 / Math.PI;

    // 辅助类
    private struct Point2D { public double X, Y; public Point2D(double x, double y) => (X, Y) = (x, y); }
    private struct Vector3D { public double X, Y, Z; public Vector3D(double x, double y, double z) => (X, Y, Z) = (x, y, z); }
    private struct GeoPoint { public double Longitude, Latitude; public GeoPoint(double lon, double lat) => (Longitude, Latitude) = (lon, lat); }
}